#include <iostream>
#include <iterator>
#include <algorithm>
#include <list>

int main()
{
  std::list<int> data;

  int n;
  std::cout << "Enter a sequence of binary digits:" << std::endl;
  std::cin >> n;
  while (n == 0 || n == 1)
    {
      data.push_back(n);
      std::cin >> n;
    }

  std::list<int> stuffed_data;
  stuffed_data = data;

  std::cout << "Input: " << std::endl;
  std::copy(data.begin(), data.end(), std::ostream_iterator<int>(std::cout, ""));
  std::cout << std::endl;

  std::list<int>::iterator it = stuffed_data.begin();
  int count = 0;

  while (it != stuffed_data.end())
    {
      if (*it == 1)
	{
	  count++;
	}
      else
	{
	  count = 0;
	}

      ++it;      
      if (count == 5)
	{
	  stuffed_data.insert(it, 0);
	  count = 0;
	}
    }

  std::cout << "After bit stuffing: " << std::endl;
  std::copy(stuffed_data.begin(), stuffed_data.end(), std::ostream_iterator<int>(std::cout, ""));
  std::cout << std::endl;


  count = 0;
  it = stuffed_data.begin();
  std::list<int>::iterator jt;
  while (it != stuffed_data.end())
    {
      if (*it == 1)
	{
	  count++;
	}
      else
	{
	  count = 0;
	}

      if (count == 5)
	{
	  jt = it;
	  ++it;
	  if (it == stuffed_data.end()) 
	    {
	      std::cout << "ERROR: Reached end of data with 5 consecutive 1s." << std::endl;
	      return 1;
	    }
	  
	  if (*it != 0)
	    {
	      std::cout << "ERROR: No zero after 5 consecutive 1s." << std::endl;
	      return 1;
	    }

	  stuffed_data.erase(it);
	  it = jt;
	  count = 0;
	}
      ++it;
    }

  std::cout << "Unstuffed data:" << std::endl;
  std::copy(stuffed_data.begin(), stuffed_data.end(), std::ostream_iterator<int>(std::cout, ""));
  std::cout << std::endl;  

  if (stuffed_data == data)
    {
      std::cout << "Unstuffing done, GOOD result!" << std::endl;
    }
  else
    {
      std::cout << "Unstuffing done, BAD result!" << std::endl;
    }
}
